// File:          my_controller.cpp
// Date:   11.30
// Description:
// Author:  baolan
// Modifications:

// You may need to add webots include files such as
// <webots/DistanceSensor.hpp>, <webots/Motor.hpp>, etc.
// and/or to add some other includes

#include <motionControl.hpp>
#include <robot_locomotion.h>

#include <ctime>
#include <webots/Keyboard.hpp>
#include <fstream>
#define PI 3.1415926

// All the webots classes are defined in the "webots" namespace
using namespace webots;
MotionControl mc;
rl rl1;

int main(int argc, char **argv) {
    Robot *robot = new Robot(); 
    Keyboard kb;
    kb.enable(1000);
    float timePeriod = 0.01;
    mc.MotionContr(timePeriod, robot);
    Camera *camera;
    camera=robot->getCamera("camera");
    camera->enable(1000);
    int timeStep = (int)robot->getBasicTimeStep();
    Matrix<float, 4, 3> initPos;
    initPos<<  0.0261167 ,-5.95758e-06  ,  -0.640955, 0.026106, -6.00657e-06 ,   -0.640965, 0.026248, -5.66663e-06 ,  -0.64084, 0.0262556 , 4.52107e-06,  -0.640832;
    mc.setInitPos(initPos);
    mc.swingFlag = 0;
    char StateVal;
    StateVal = mc.stateval[0];
    // ofstream oFile;
    // oFile.open("force.csv",ios::out | ios::trunc);
  while (robot->step(timeStep) != -1)
  {
    mc.key = kb.getKey();
    mc.Sensor_update();
    mc.setCoMVel(mc.tCV);
    mc.forwardKinematics();
    mc.jacobians();
    mc.state_judgement();



    rl1.dsensor[0] =  mc.distance[0];
    rl1.dsensor[1] =  mc.distance[1];
    rl1.dsensor[2] =  mc.distance[2];
    rl1.dsensor[3] =  mc.distance[3];
    rl1.dsensor[4] =  mc.distance[4];
    rl1.dsensor[5] =  mc.distance[5];

    rl1.run();
    rl1.update();
    
    mc.tCV[0] = rl1.vx;
    mc.tCV[1] = rl1.vy;
    mc.tCV[2] = rl1.w;

      
    // clock_t startTime,endTime;
    // startTime = clock();//计时开始
    switch(StateVal)
    {
      case 'a'://
         mc.State_start();
         if (mc.Start_time >= 0.20)
         {   
           StateVal = mc.stateval[1];
           // mc.Start_time = 0.0;
           mc.fly_time = 0.0;
         }
         cout << "a"<<endl;
         break;
      case 'b':// 2 leg stance
        mc.stance_VMC();
        mc.swing_VMC();
        mc.Setjoint();
        // if (mc.fly_time > mc.T - 0.0005 && mc.state_val == 1)
        //   StateVal = mc.stateval[3];
        // else if (mc.fly_time > mc.T - 0.0005 && mc.state_val == 2)
        //   StateVal = mc.stateval[2];
        cout << "b"<< endl;
        break;
      case 'c'://3 leg stance
        mc.stance_VMC_3leg();
        mc.swing_VMC_3leg();
        mc.Setjoint();
        if (mc.fly_time > mc.T - 0.0005 || mc.state_val == 1)
          StateVal = mc.stateval[3];
        cout << "c"<< endl;
        break;
      case 'd'://4 leg stance
        mc.stance_VMC_4leg();
        mc.Setjoint();
        if (mc.swingFlag == 0)
        {
          mc.swingFlag = 1;
          mc.fly_time = 0.0;
        }
        else
        {
          mc.swingFlag = 0;
          mc.fly_time = 0.0;
        }
        StateVal = mc.stateval[1];
        // oFile << mc.leg_force[0]<<","<< mc.leg_force[1]<<","<< mc.leg_force[2]<<","<< mc.leg_force[3]<<","<< mc.leg_force[4]<<","<< mc.leg_force[5]<<","<< mc.leg_force[6]<<","<< mc.leg_force[7]<<","<< mc.leg_force[8]<<","<< mc.leg_force[9]<<","<< mc.leg_force[10]<<","<< mc.leg_force[11]<< endl;
        // cout << "d" <<endl;
        break;
      default:
      break;
    }
    cout << "dis:   "<<mc.distance.transpose()<< endl;
    // cout << "v:   "<<mc.tCV.transpose()<<endl;
  }
// oFile.close();
delete robot;
return 0;
}
